﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Diagnostics;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Media;

namespace Engine.Physics
{
    enum CollissionBounds
    {
        Box,
        Sphere,
        ShapeApproximation
    }

    public class CollisionInfo
    {
        public Collider collider1;
        public Collider collider2;
        public Vector2 MTV;
        public Vector2 Normal;
        public List<Vector2> ContactPoints;
        public bool collding = false;
        public bool willCollide = false;

        public CollisionInfo (Collider _coll1, Collider _coll2, Vector2 _projection)
	    {
            collider1 = _coll1;
            collider2 = _coll2;
            MTV = _projection;
            ContactPoints = new List<Vector2>();
	    }

        public override bool Equals(object obj)
        {
            if (obj is CollisionInfo)
                return ((obj as CollisionInfo).collider1.Equals(collider1) && (obj as CollisionInfo).collider2.Equals(collider2)) ||
                       ((obj as CollisionInfo).collider2.Equals(collider1) && (obj as CollisionInfo).collider1.Equals(collider2));
            return base.Equals(obj);
        }

        public override string ToString()
        {
            return "Collider 1: " + collider1.gameObject.Name + "; " + "Collider 2: " + collider2.gameObject.Name + "; " + MTV.ToString();
        }
    }

    public class PhysicsEngine : GameComponent
    {
        public Vector2 Gravity = new Vector2(0, 200);
        public const float COF = 0.5f;
        public const float SCOR = 0.5f;

        public bool IsActive = false;

        public static PhysicsEngine Instance { get { return mInstance; } }
        private static PhysicsEngine mInstance;

        private List<Rigidbody> rigidbodies;
        private List<ForceField> forceFields;
        private List<Collider> colliders;
        private List<CollisionInfo> collisions;

        private BroadPhaseAlgorithm BroadPhase = new SweepAndPrune();


        public PhysicsEngine(Game _game)
            : base(_game)
        {
            mInstance = this;
            rigidbodies = new List<Rigidbody>();
            forceFields = new List<ForceField>();
            colliders = new List<Collider>();
            collisions = new List<CollisionInfo>();
        }

        public void Reset()
        {
            rigidbodies.Clear();
            forceFields.Clear();
            colliders.Clear();
        }
        
        public void AddForceField(ForceField _field)
        {
            forceFields.Add(_field);
        }

        public void RemoveForceField(ForceField _field)
        {
            forceFields.Remove(_field);
        }

        public void AddRigidbody(Rigidbody _body)
        {
            rigidbodies.Add(_body);
        }

        public void RemoveRigidbody(Rigidbody _body)
        {
            rigidbodies.Remove(_body);
        }

        public void AddCollider(Collider _collider)
        {
            colliders.Add(_collider);
            BroadPhase.NotifyColliderAdded(colliders);
        }

        public void RemoveCollider(Collider _collider)
        {
            colliders.Remove(_collider);
            BroadPhase.NotifyColliderRemoved(colliders);
        }

        public override void Update(GameTime gameTime)
        {
            float timeDiff = (float)gameTime.ElapsedGameTime.TotalSeconds;

            //(BroadPhase as AABBTree).Draw();

            StartFrame();
            Integrate(timeDiff);
            BroadPhase.NotifyColliderAdded(colliders);
            List<CollisionPair> pairs = BroadPhase.FindPairs(colliders);
            Graphics.GraphicsEngine.Instance.DrawString(pairs.Count.ToString(), new Vector2(20, 20), Color.Black);
            Graphics.GraphicsEngine.Instance.DrawString(colliders.Count.ToString(), new Vector2(20, 50), Color.Black);
            collisions = CollisionCheck(timeDiff, pairs);

            CollisionResponse();

            foreach (Rigidbody body in rigidbodies)
                body.EndFrame();

            base.Update(gameTime);
        }

        #region Private Methods

        private void StartFrame()
        {
            foreach (Rigidbody body in rigidbodies)
            {
                if (body.IsActive && body.IsAffectedByGravity)
                    body.Accelerate(Gravity);
                body.StartIntegration(); 
            }
        }

        private void Integrate(float _timeDiff)
        {
            foreach (Rigidbody body in rigidbodies)
            {
                if (!body.IsActive)
                    continue;
                body.IntegrateTimestep(_timeDiff);
                if (body.gameObject != null && body.gameObject.collider != null)
                {
                    body.gameObject.collider.RecalcShape();
                }
            }
        }

        private List<CollisionInfo> CollisionCheck(float _timeDiff, List<CollisionPair> _pairs)
        {
            List<CollisionInfo> returnColls = new List<CollisionInfo>();

            foreach (CollisionPair pair in _pairs)
            {
                Collider coll1 = pair.Coll1;
                Collider coll2 = pair.Coll2;

                if (coll1.IsStatic && coll2.IsStatic || (!coll1.IsActive ||!coll2.IsActive))
                    continue;

                CollisionInfo collInfo;
                coll1.Intersects(coll2, out collInfo);

                if (collInfo != null && collInfo.collding)
                {
                    if (!returnColls.Contains(collInfo))
                    {
                        returnColls.Add(collInfo);
                        coll1.gameObject.OnCollision(collInfo);
                        collInfo.collider1 = coll2;
                        collInfo.collider2 = coll1;
                        collInfo.MTV *= -1;

                        coll2.gameObject.OnCollision(collInfo);
                        collInfo.collider1 = coll1;
                        collInfo.collider2 = coll2;
                        collInfo.MTV *= -1;
                    }
                }
            }
            return returnColls;
        }

        private void ResolveCollision(Rigidbody _b1, Rigidbody _b2, Vector2 _contact1, Vector2 _contact2, Vector2 _relV, Vector2 _collNormal)
        {
            Vector2 vt = _relV - Vector2.Dot(_relV, _collNormal) * _collNormal;
            Vector2 frictionNormal = -vt; frictionNormal.Normalize();

            //Rename variables to be more convenient
            float m1 = _b1 != null ? _b1.Mass : 0;
            float m2 = _b2 != null ? _b2.Mass : 0;
            float inverseM1 = _b1 != null ? 1f / m1 : 0;
            float inverseM2 = _b2 != null ? 1f / m2 : 0;

            float COR1 = _b1 != null ? _b1.COR : SCOR;
            float COR2 = _b2 != null ? _b2.COR : SCOR;

            float inverseInertia1 = _b1 != null ? 1f / _b1.Inertia : 0;
            float inverseInertia2 = _b2 != null ? 1f / _b2.Inertia : 0;

            float e = (COR1 + COR2) / 2;

            float crossA1 = MathUtils.Cross2D(_contact1, _collNormal);
            float crossB1 = MathUtils.Cross2D(_contact2, _collNormal);

            float dot = inverseInertia1 * crossA1 * crossA1 +
                        inverseInertia2 * crossB1 * crossB1;

            //Collision Impulse
            float jc = Vector2.Dot(_relV, _collNormal) / (inverseM2 + inverseM1 + dot);

            //Friction Impulse
            float jf = Vector2.Dot(_relV, frictionNormal) / ((inverseM1 + inverseM2) +
                        MathUtils.Cross2D(_contact1, frictionNormal) * MathUtils.Cross2D(_contact1, frictionNormal) * inverseInertia1 +
                        MathUtils.Cross2D(_contact2, frictionNormal) * MathUtils.Cross2D(_contact2, frictionNormal) * inverseInertia2);

            //Clamp friction impulse
            if (Math.Abs(jf) > Math.Abs(jc * COF))
                jf = Math.Abs(COF) * Math.Sign(jc);

            //Final impulse magnitude
            float j = (jc * -(e + 1));

            //Final impulse vector
            Vector2 imp = j * _collNormal + (vt != Vector2.Zero ? frictionNormal * (-jf) : Vector2.Zero);

            if (_b1 != null)
            {
                float cross1 = MathUtils.Cross2D(_contact1, imp);
                _b1.newAngularVelocity += inverseInertia1 * cross1;
                _b1.newVelocity += imp / m1;
            }

            if (_b2 != null)
            {
                float cross2 = MathUtils.Cross2D(_contact2, imp);
                _b2.newAngularVelocity -= inverseInertia2 * cross2;
                _b2.newVelocity += -imp / m2;
            }
            
        }

        private void CollisionResponse()
        {
            foreach (CollisionInfo collision in collisions)
            {
                if (collision.ContactPoints.Count == 0)
                    continue;

                //Move apart
                if (collision.collider1.gameObject.rigidbody != null)
                    collision.collider1.gameObject.rigidbody.newPosition += collision.MTV / 3;
                else if (collision.collider2.gameObject.rigidbody != null)
                    collision.collider2.gameObject.rigidbody.newPosition -= collision.MTV / 3;
                    
                if (collision.collider2.gameObject.rigidbody != null)
                    collision.collider2.gameObject.rigidbody.newPosition -= collision.MTV / 3;
                else if (collision.collider1.gameObject.rigidbody != null)
                    collision.collider1.gameObject.rigidbody.newPosition += collision.MTV / 3;

                Vector2 joinedContactPoint = new Vector2(0);
                foreach (Vector2 contactPoint in collision.ContactPoints)
                    joinedContactPoint += contactPoint;
                joinedContactPoint /= collision.ContactPoints.Count;

                Vector2 conPoint1 = joinedContactPoint - collision.collider1.Position;
                Vector2 conPoint2 = joinedContactPoint - collision.collider2.Position;
                    
                Vector2 relV = Vector2.Zero;
                if (collision.collider1.gameObject.rigidbody != null)
                    relV = collision.collider1.gameObject.rigidbody.newVelocity + collision.collider1.gameObject.rigidbody.newAngularVelocity * MathUtils.Cross2D(conPoint1);

                if (collision.collider2.gameObject.rigidbody != null)
                    relV -= collision.collider2.gameObject.rigidbody.newVelocity + collision.collider2.gameObject.rigidbody.newAngularVelocity * MathUtils.Cross2D(conPoint2);

                ResolveCollision(collision.collider1.gameObject.rigidbody, collision.collider2.gameObject.rigidbody, conPoint1, conPoint2, relV, collision.Normal);
            }
        }
        #endregion
    }
}
